/*
 * user_code.cpp
 *
 *  Created on: Jul 8, 2021
 *      Author: xrj
 */

#include "user_code.hpp"
#include "mylibs_config.hpp"
#include "stm32f1xx_hal.h"
#include "cmsis_os.h"
#include "step_motor.hpp"
#include "c_adc.hpp"
#include "c_usbhid.hpp"
#include "sender.hpp"
#include "delay.hpp"

extern TIM_HandleTypeDef htim2;
C_Pin pins[4] = {{SM1_GPIO_Port, SM1_Pin}, {SM2_GPIO_Port, SM2_Pin},
		         {SM3_GPIO_Port, SM3_Pin}, {SM4_GPIO_Port, SM4_Pin}};
GPIO_Conn sm_conn = GPIO_Conn(pins, 4, true, InitCfg_Disable, NoLock, Pin_PP0, Pin_PP0);
C_TIM ctim2 = C_TIM(&htim2);
StepMotor sm = StepMotor(sm_conn, &ctim2);

extern TIM_HandleTypeDef htim4;
extern ADC_HandleTypeDef hadc1;
C_TIM ctim4 = C_TIM(&htim4);
C_ADC cadc = C_ADC(&hadc1);
Buffer buff = Buffer(2);

extern USBD_HandleTypeDef hUsbDeviceFS;
USBHID *usb = (USBHID*)&hUsbDeviceFS;

uint32_t *trig = C_Pin(US_Trig_GPIO_Port, US_Trig_Pin).ODR_bitband();
uint32_t *echo = C_Pin(US_Echo_GPIO_Port, US_Echo_Pin).IDR_bitband();

void Thread()
{
	sm.Init();
	sm.setMode(StepMotor_8);
	int steps=1024;
	for(;;){
		int state = sm.get_rot_state();
		if(state>=30000){
			steps=-1024;
		}if(state<=-30000){
			steps=1024;
		}
		sm.run_us(1000, steps, true);
	}
}
